; Use Basic Micro ATOM PRO IDE ver 8.0.1.0

'******************** IDE PROBLEMS ***************************************
'Using replacement Constants for input/output pins does NOT work!  Must use the 
' predefined (in0, etc...) constants instead.
'The COS function does not work correctly use - SIN(angle + 64) instead
'The SQR function does not work correctly use - SQR(x) & 0x7FFF
'The PULSIN function does not work correctly use - RCTIME
'NOTE: P6 and P7 can input 5vdc but only swing from 0 to 3.3vdc for outputs!! 
'NOTE: AX0-AX3 are identified as P16-P19 and NOT in order!!
'There is a bug in the compiler!!!  The only way around the bug is to do 0 + constant
'instead of just assigning the constant.  Note that it only surfaces on occassion.
'******************** END IDE PROBLEMS ************************************

'******************************* Bot Board Pin Assignments *****************************
'Bot Board Communications
'CONSTANTS
'the following analog pin identifiers for the PRO was  reported by Tyson on the Lynxmotion
'forums and was verified on 10/19/2007 at 2:06 am.  This is undocumented!  Yes they really
'are out of sequence!  Argh!
VS_Sense con P17 ': pin AX0 (analog) servo voltage sensing
'unused con P16 ': pin AX1 (analog) logic voltage sensing
'UNUSED con P18 'AX2 (analog)
'UNUSED con P19 'AX3 (analog)
 
 'DualShock
'		    the yellow and black pair are positive 5vdc and ground respectively  
'UNUSED con P0 'Data from PS2 controller 012,201    orange
'UNUSED con P1 'Command to PS2 controller			brown
'UNUSED con P2 'Select to PS2 controller			blue
'UNUSED con P3 'Clock pulse to PS2 controller		black
RED_PB con P4 '  
GREEN_PB con P5
YELLOW_PB con P6 'Purple WILL NOT DRIVE A SERVO! This swings only to 3.3v on the Atom Pro!
'UNUSED con P7 'Blue WILL NOT DRIVE A SERVO! This swings only to 3.3v on the Atom Pro!
'UNUSED con P8 'Yellow
Speaker con P9 
'UNUSED con P10 
CMUCamReq  con P11 
'unused con P12  
SVA con P13  
SVB con P14 '(SCI3 RXD) 
SVC con P15 '(SCI3 TXD)  
;******************************* End Bot Board Pin Assignments *************************

;-------------Constants

SVA_OFF con -200 'offset for servo A
SVB_OFF con -150 'offset for servo B 165 cw  150ccw
SVC_OFF con -200 'offset for servo C

TRUE con 1 'used for logic
FALSE con 0 'used for logic

pb_delay con 80
pb_rate con 40

mode_center con 0
mode_clockwise con 1
mode_cclockwise con 2
mode_run con 5


;--------------------------------------------------------------------				
;-------------Variables

'when viewed clocwise above camera
ServoA var sword 
ServoB var sword 
ServoC var sword

count1 var byte 'for loop counting

'for voltage sensing
V_Raw var word 'voltage raw data
VS_F var float 'converted servo voltage to floating point

'for holonomic drive
i var byte 						'used for for loops
temp2 var float
vel var float
sHead var float

radius var float 'in meters
Fx var float(3)
Fy var float(3)

baseline var float 'in meters (1 inch = 0.0254 meters)
vb_x var float 'desired body velocity x
vb_y var float 'desired body velocity y
vb_w var float 'body angular velocity
W var float(3)  'wheel angular velocity - 'rotational velocity of each wheel

tempvel var float
velVal var sword

mysin var float
mycos var float

fpunits var byte
tmpSin var sbyte
tmpCos var sbyte

vectemp var byte
	
'for modes
botmode var byte
  
;--------------------------------------------------------------------	
;***************
;*** Program ***
;***************

;-------------Init
ENABLEHSERVO  

clear

botmode = 0

'using variables here instead of constants because of compiler complaints
'when handling a floating point constant
radius = 0.0254 '1 inch converted to meters

Fx(0) = -1.0
Fy(0) = 0.0

Fx(1) = 0.5
Fy(1) = -0.866

Fx(2) = 0.5
Fy(2) = 0.866

baseline = 0.0762 '3 inch baseline (6 inch diameter)
vb_x = 0.0
vb_y = 0.0
vb_w = 0.0

Sound Speaker,[100\4235, 200\4435]

'NOTE:  NEVER MAKE THE PB'S HIGH OUTPUTS TO TURN OFF LED'S - MAKE THEM INPUTS
'INSTEAD
low RED_PB  'NEVER MAKE A PB HIGH!
low YELLOW_PB  'NEVER MAKE A PB HIGH!
low GREEN_PB  'NEVER MAKE A PB HIGH!

gosub CenterServos
pause 1000
sHead = 0.0  'degree heading
'now we are ready to run

;--------------------------------------------------------------------
;-------------Main loop
main				

	gosub GetVoltage
	if VS_F < 20.0 then
		low RED_PB  'NEVER MAKE A PB HIGH!
		low YELLOW_PB  'NEVER MAKE A PB HIGH!
		low GREEN_PB  'NEVER MAKE A PB HIGH!
		pause 250
	endif
	
	input RED_PB
	input YELLOW_PB
	input GREEN_PB
	count1 = 0
	button RED_PB, 0, pb_delay, pb_rate, count1, 1, SELECTRED 
	count1 = 0
	button YELLOW_PB, 0, pb_delay, pb_rate, count1, 1, SELECTYELLOW 
	count1 = 0
	button GREEN_PB, 0, pb_delay, pb_rate, count1, 1, SELECTGREEN

	if botmode = mode_center then
		gosub CenterServos
	elseif botmode = mode_clockwise 		
		gosub BotClockwise
	elseif botmode = mode_cclockwise
		gosub BotCounterClockwise
	elseif botmode = mode_run
		gosub HoloServo
	endif
	
	pause 100 'need to delay the loop a little bit
	goto main 'DUH!  NEED THIS OR WE GET FALL-THROUGH

SELECTRED	
	Sound Speaker,[100\2235, 200\2435]
	low RED_PB  'NEVER MAKE A PB HIGH!
	botmode = mode_center
	goto main
		
'this section changes the heading		
SELECTYELLOW
	Sound Speaker,[100\3235, 200\3435]
	low YELLOW_PB  'NEVER MAKE A PB HIGH!
	
	sHead = sHead + 10.0
		
	if sHead > 359.0  then
		sHead = 0.0
	endif
	goto main
	
SELECTGREEN
	Sound Speaker,[100\5235, 200\5435]
	low GREEN_PB  'NEVER MAKE A PB HIGH!
	botmode = mode_run
	goto main	
	
	'this goto main is here just in case as a reminder that this is the end
	'of the main program
	goto main

'*********************** End Main *************************

'*********************** Subroutines **********************

CenterServos
	ServoA = 0 - SVA_OFF
	ServoB = 0 - SVB_OFF
	ServoC = 0 - SVC_OFF

	HSERVO [SVA\ServoA\255, SVB\ServoB\255, SVC\ServoC\255]	
return


BotClockwise
	ServoA = 1000 - SVA_OFF
	ServoB = 1000 - SVB_OFF
	ServoC = 1000 - SVC_OFF	 
	HSERVO [SVA\ServoA\255, SVB\ServoB\255, SVC\ServoC\255] 
return


BotCounterClockwise
	ServoA = -1000 - SVA_OFF
	ServoB = -1000 - SVB_OFF
	ServoC = -1000 - SVC_OFF	 
	HSERVO [SVA\ServoA\255, SVB\ServoB\255, SVC\ServoC\255] 
return

'*************************** Subroutine GetVoltage ********************
GetVoltage	
	'*** Get VS voltage to see if we need a new battery yet
	adin VS_Sense, V_Raw
	VS_F = tofloat(V_Raw) * 0.005 * 4.0 'converts the 0-1023 input to volts	
return


HoloServo
	if sHead > 359.0 then
		sHead = 0.0
	endif
	
	fpunits = TOINT sHead * 256/360  ;convert heading to 0-255 for functions
	tmpSin = SIN(fpunits) ;take the SIN of the degrees to get X
	mysin = TOFLOAT tmpSin/128.0 ;convert X to radians/sec
	vb_x = mysin * 0.1  ;scale X
	tmpCos = COS(fpunits)   ;take the COS of the degrees to get Y
	mycos = TOFLOAT tmpCos / 128.0  ;convert Y to radians/sec
	vb_y  = mycos * 0.1		;scale Y
	
	gosub Vector_Drive
    	
return


' Drives robot in a given direction vector V with given angular
' velocity W.
' r - wheel radius in meters
' b - wheel baseline in meters
' ||vb_x|| must be max of 0.1 and not negative!
' ||vb_y|| must be max of 0.1 and not negative!
Vector_Drive

	for vectemp = 0 to 2
		W(vectemp) = ((Fx(vectemp) * vb_x + Fy(vectemp) * vb_y) + baseline * vb_w)/radius 	
   	next
   	
   	' makes sure that given path is physically possible
	if ((W(0)>6.0) or (W(0)>6.0) or (W(1)>6.0) or (W(1)<-6.0) or (W(2)<-6.0) or (W(2)<-6.0)) then
		'light red and yellow lights to indicate a problem	
		low YELLOW_PB  'NEVER MAKE A PB HIGH!
		low RED_PB  'NEVER MAKE A PB HIGH!	
		pause 500
		input RED_PB
		input YELLOW_PB
	else
		vel = W(0)
		gosub Vel_To_Value
		ServoA = velVal
		
		vel = W(1)
		gosub Vel_To_Value
		ServoB = velVal
		
		vel = W(2)
		gosub Vel_To_Value
		ServoC = velVal
		
		'for troubleshooting only	
		'serout S_OUT, i38400,["sHead: ", real sHead, "  A: ", dec ServoA, "   B: ", dec ServoB, "  C: ", dec ServoC ,13]
	   'serout S_OUT, i38400,["sHead: ", real sHead, "  mysin: ", real mysin, "  mycos: ", real mycos, "  tSin: ", dec tmpSin, "  tCos: ", dec tmpCos,13]

		'transmit command to servos	
		ServoA = ServoA - SVA_OFF
		ServoB = ServoB - SVB_OFF
		ServoC = ServoC - SVC_OFF	 
		HSERVO [SVA\ServoA\255, SVB\ServoB\255, SVC\ServoC\255] 
	endif
return


' Calculates servo value using Cubic Regression y=a*x^3+b*x^2+c*x+d
' Converts velocity in rad per second to servo value (-12000 to 12000)
' vel can only be [-6,6] rad/sec
' x can only be [-1,1] rev/sec
Vel_To_Value
	temp2 = vel / 3.154 / 2.0	' convert rad/sec to rev/sec
	if ((vel > -6.0) and (vel < 6.0)) then
		'use for 0-255 range
		tempvel = (39.8053*temp2*temp2*temp2 - 12.6083*temp2*temp2 + 22.1197*temp2 + 128.4262)
		tempvel = tempvel - 127.0 ;scale is now -127 to 127
		tempvel = tempvel * 94.48  ;scale is now -12000 to 12000 
		;for -12000 to 12000  tempvel = (6500.0*temp2*temp2*temp2 - 12.0*temp2*temp2 + 6500.0*temp2 + 0.0)
		velVal = TOINT tempvel
	else
		velVal = 0 
	endif
return




'************** END PROGRAM ****************************